/*
 * Generic PID controler class
 */

#include "pid.h"

namespace control_test {
    PID::PID(int h_, int K_, int Ti_, int Td_, int b_),prev_output(0), ud(0), ui(0),h(h_), K(K_), Ti(Ti_), Td(Td_), b(b_) {
        // Initialize the PID with all the required values
        Tt = Ti;    // or Tt = 0.1*Ti to 0.5*Ti
        bi = K * h / Tt;
        ad = Td / (Td + N*h);
        bd = (K*Td*N) / (Td + N*h);
        a0 = h/Tt;
    }

    int PID::Update(int target, int current) {
        // Update the PID and return the control signal
        // Compute the proportional part
        up = K*(b*target - current);
        // Update the derivative part
        ud = ad*ud - bd*(current - prev_output);
        // Compute control variable (v)
        v = up + ui + ud;
        // Limit the control variable (u)
        if (v > umax) { u = umax; }
        else if (v < umin) { u = umin; }
        else { u = v; }
        // Update the integral part
        ui = ui + bi*(target-current) + a0*(u - v);
        
        return u;
    }

}
